/*
 * AutoReferee.cpp
 *
 *  Created on: 2011-03-20
 *      Author: kamil
 */

#include "AutoReferee.h"
#include "common.h"
#include "mgr/config/Config.h"
#include "mgr/id/IdMgr.h"

using namespace boost::posix_time;
using namespace std;

log4cxx::LoggerPtr AutoReferee::logger(log4cxx::Logger::getLogger("AutoReferee"));

AutoReferee::AutoReferee(boost::asio::io_service & svc, const CmdLineParser & cfg) :
	io_service(svc), videoClient(io_service),
	server(cfg.getRefereeIp(), cfg.getRefereePort(), svc),
	state(S_PRE_FIRST_HALF),
	firstGoalOwners(TEAM_UNDEFINED),
	secondGoalOwners(TEAM_UNDEFINED),
	goalScored(false)
{
	memset(&currentCmdPacket, 0, sizeof(currentCmdPacket));
	videoClient.connect(cfg.getVideoServerIp(), cfg.getVideoServerPort());

	currentCmdPacket.cmd = Cmd::STOP;
	lastMulticastTime = boost::posix_time::second_clock::universal_time();
}

AutoReferee::~AutoReferee()
{

}

void AutoReferee::run(){
	//just some delay at the beginning, it will also cause first multicast to be send (multicast_period will elapse)
	sleep(1);
	send();	//sending stop command


	while(true){
		size_t packetsCount = io_service.run_one();	//this is blocking!
		LOG4CXX_TRACE(logger, "Packets received: "<<packetsCount);
		if (packetsCount && videoClient.areNewDataPresent()){
			LOG4CXX_TRACE(logger, "New data are present, starting calculations");
			//calculations of game state, they may current ref.dec
			map<string, Videodata> tmpMap;
			double timeStamp;	//not used, simulation time
			videoClient.consumeVideodata(&tmpMap, &timeStamp);
			map<string, Videodata>::iterator it = tmpMap.begin();
			for(; it != tmpMap.end(); it++){
				modelsData[IdMgr::instance().getId(it->first)] = it->second;
			}

			if (!modelsData.empty() && firstGoalOwners == TEAM_UNDEFINED)
				calculateGoalOwners();

			gameInfo = videoAnalyzer.analyze(modelsData);
			LOG4CXX_DEBUG(logger, "******** CURRENT COMMAND: "<<currentCmdPacket.cmd<<" ***********");
			LOG4CXX_DEBUG(logger, "BALL moved: "<<gameInfo.ballMoved<<" owned: "<<gameInfo.ballOwned<<" playersMoving: "<<gameInfo.playersMoving);
			if (gameInfo.lastBallContact != ModelId::UNDEFINED){
				LOG4CXX_DEBUG(logger, "lastTouched: "<<IdMgr::instance().getName(gameInfo.lastBallContact ));
			}
			else{
				LOG4CXX_DEBUG(logger, "lastTouched: UNDEFINED");
			}
			updateState();
		}

		multicast();
	}
}

void AutoReferee::updateState(){
	RefereeState newState = state;
	char newCommand;

	switch (state) {
	case S_PRE_FIRST_HALF: {
		if (rand() % 2)
			newCommand = Cmd::KICKOFF_B;
		else
			newCommand = Cmd::KICKOFF_Y;

		newState = S_KICKOFF;
		videoAnalyzer.saveBallPosition(getPosition(Config::getBallInfo().id).pos);
	} break;

	case S_GOAL: {
		BOOST_ASSERT(ballOutDecision.cmd == Cmd::GOAL_B || ballOutDecision.cmd == Cmd::GOAL_Y);
		if (ballOutDecision.cmd == Cmd::GOAL_B){
			newCommand = Cmd::KICKOFF_Y;
			newState = S_KICKOFF;
		}
		else if (ballOutDecision.cmd == Cmd::GOAL_Y){
			newCommand = Cmd::KICKOFF_B;
			newState = S_KICKOFF;
		}
		goalScored = true;

	}	break;

	case S_KICKOFF: {
		//if robot accidentally moved ball or all robots are in place and ball is taken to be kicked

		if (!validateRobotPositions()) break;

		if (gameInfo.ballMoved || (gameInfo.ballOwned && !gameInfo.playersMoving)){
			LOG4CXX_DEBUG(logger, "Moving from Kickoff to ready");
			LOG4CXX_DEBUG(logger, "ballMoved: "<<gameInfo.ballMoved);
			LOG4CXX_DEBUG(logger, "ball owned: "<<gameInfo.ballOwned);
			LOG4CXX_DEBUG(logger, "players moving: "<<gameInfo.playersMoving);
			newState = S_READY;
			newCommand = Cmd::READY;
		}
	} break;

	case S_READY: {
		goalScored = false;
		if (gameInfo.ballOut){
			newState = S_BALL_PLACEMENT;
			newCommand = Cmd::STOP;
			//placementInfo.lastGameInfo = gameInfo;
			placementStart = second_clock::universal_time();
			//calculatePlacementPosition();

			ballOutDecision = calculateBallOutDecision();
			LOG4CXX_DEBUG(logger, "BallOutDecision command: "<<ballOutDecision.cmd);
		}
	} break;

	case S_BALL_PLACEMENT: {
		time_duration diff = second_clock::universal_time() - placementStart;
		if (diff.total_milliseconds() > PLACEMENT_DELAY){
			gazeboClient.setBallPosition(ballOutDecision.ballPos.x, ballOutDecision.ballPos.y, 0.1);

			if (ballOutDecision.cmd == Cmd::GOAL_B || ballOutDecision.cmd == Cmd::GOAL_Y)
				newState = S_GOAL;
			else
				newState = S_KICKOFF;

			newCommand = ballOutDecision.cmd;
			videoAnalyzer.saveBallPosition(getPosition(Config::getBallInfo().id).pos);
		}
		else{
			gazeboClient.setBallPosition(ballOutDecision.ballPos.x, ballOutDecision.ballPos.y, 0.3);
		}

	} break;

	default:
		BOOST_ASSERT(false); //unsupported state
		break;
	}

	if (newState != state) {
		state = newState;
		createCommand(newCommand);
		send();
	}
}

void AutoReferee::createCommand(char newCommand){
	currentCmdPacket.cmd_counter++;
	currentCmdPacket.cmd = newCommand;
}

AutoReferee::BallOutDecision AutoReferee::calculateBallOutDecision(){
	BallOutDecision decision;

	LOG4CXX_DEBUG(logger, "calculateBallOutDecision");
	LOG4CXX_DEBUG(logger, "Out pos: "<<gameInfo.outPosition);
	const double PLACEMENT_OFFSET = 0.5;
	Vector2d pos = gameInfo.outPosition;
	Rect gameArea = Config::getFieldInfo().gameArea;
	Vector2d firstGoalPos = Config::getFieldInfo().firstGoal;
	Vector2d secondGoalPos = Config::getFieldInfo().secondGoal;
	double goalWidth2 = Config::getFieldInfo().goalWidth / 2.0;

	//////////////////////////////////////////////////////////
	/////////////////////// ball out /////////////////////////
	//////////////////////////////////////////////////////////

	if (pos.x < gameArea.x || pos.x > gameArea.x + gameArea.w){
		if (Config::instance().getTeamName(gameInfo.lastBallContact) == TEAM_BLUE)
			decision.cmd = Cmd::KICKOFF_Y;
		else
			decision.cmd = Cmd::KICKOFF_B;

		if (pos.x < gameArea.x)
			decision.ballPos.x = gameArea.x + PLACEMENT_OFFSET;
		else
			decision.ballPos.x = gameArea.x + gameArea.w - PLACEMENT_OFFSET;

		decision.ballPos.y = pos.y;
		if (pos.y < gameArea.y + PLACEMENT_OFFSET) decision.ballPos.y = gameArea.y + PLACEMENT_OFFSET;
		if (pos.y > gameArea.y + gameArea.h - PLACEMENT_OFFSET) decision.ballPos.y = gameArea.y + gameArea.h - PLACEMENT_OFFSET;

		return decision;
	}

	////////////////////////////////////////////////////////////////////
	///////////////////////////// goal /////////////////////////////////
	////////////////////////////////////////////////////////////////////

	if (fabs(pos.x - firstGoalPos.x) < goalWidth2){
		if (pos.y < gameArea.y){
			//goal scored into the first goal!
			if (firstGoalOwners == TEAM_BLUE)
				decision.cmd = Cmd::GOAL_Y;
			else
				decision.cmd = Cmd::GOAL_B;
		}
		else{	//goal scored to the second goal!
			if (secondGoalOwners == TEAM_BLUE)
				decision.cmd = Cmd::GOAL_Y;
			else
				decision.cmd = Cmd::GOAL_B;
		}

		decision.ballPos = Config::getFieldInfo().center;
		return decision;
	}

	////////////////////////////////////////////////////////////////////
	/////////////////// corner or goal kick ////////////////////////////
	////////////////////////////////////////////////////////////////////

	TeamName outGoalOwners;	//ball crossed the line near this goal
	if (pos.y < gameArea.y) outGoalOwners = firstGoalOwners;
	else outGoalOwners = secondGoalOwners;

	////////////////////// corner kick ///////////////////////////
	if (Config::instance().getTeamName(gameInfo.lastBallContact) == outGoalOwners){
		outGoalOwners == TEAM_BLUE ? decision.cmd = Cmd::KICKOFF_Y : decision.cmd = Cmd::KICKOFF_B;

		if (pos.x < firstGoalPos.x)
			decision.ballPos.x = gameArea.x + PLACEMENT_OFFSET;
		else
			decision.ballPos.x = gameArea.x + gameArea.w - PLACEMENT_OFFSET;

		//ballPos.y is calculated below
	}
	////////////////////// goal kick /////////////////////////////
	else{
		outGoalOwners == TEAM_BLUE ? decision.cmd = Cmd::KICKOFF_B : decision.cmd = Cmd::KICKOFF_Y;

		if (pos.x < firstGoalPos.x)
			decision.ballPos.x = gameArea.x + gameArea.w / 4.0;
		else
			decision.ballPos.x = gameArea.x + 3.0 * gameArea.w / 4.0;
		//ballPos.y is calculated below
	}

	if (outGoalOwners == firstGoalOwners)
		decision.ballPos.y = gameArea.y + PLACEMENT_OFFSET;
	else
		decision.ballPos.y = gameArea.y + gameArea.h - PLACEMENT_OFFSET;

	return decision;
}

void AutoReferee::calculateGoalOwners(){
	firstGoalOwners = Config::instance().getTeamName(getClosestRobot(Config::getFieldInfo().firstGoal));
	secondGoalOwners = Config::instance().getTeamName(getClosestRobot(Config::getFieldInfo().secondGoal));
	LOG4CXX_DEBUG(logger, "First goal owners: "<<firstGoalOwners);
	LOG4CXX_DEBUG(logger, "Second goal owners: "<<secondGoalOwners);
}

ModelId AutoReferee::getClosestRobot(Vector2d point){
	ModelId closest;
	double closestDist = std::numeric_limits<double>::max();
	Vector2d tmp;
	double tmpDist;
	for (map<ModelId, Videodata>::iterator i = modelsData.begin(); i != modelsData.end(); ++i){
		if (i->first == Config::getBallInfo().id) continue;
		tmp.x = i->second.x;
		tmp.y = i->second.y;
		tmpDist = (tmp - point).length();
		if (tmpDist < closestDist){
			closest = i->first;
			closestDist = tmpDist;
		}
	}
	return closest;
}

bool AutoReferee::validateRobotPositions(){
	bool valid = true;
	TeamName executors = TEAM_BLUE;
	if (currentCmdPacket.cmd == Cmd::KICKOFF_Y) executors = TEAM_YELLOW;

	if (goalScored){
		//all robots must be on proper field half
		for (map<ModelId, Videodata>::iterator i = modelsData.begin(); i != modelsData.end(); ++i){
			if (i->first == Config::getBallInfo().id) continue;

			if (Config::instance().getTeamName(i->first) == firstGoalOwners){
				if (i->second.y > Config::getFieldInfo().center.y){
					if (firstGoalOwners == executors){
						Vector2d modelPos(i->second.x, i->second.y);
						if ((modelPos - Config::getFieldInfo().center).length() < Config::getFieldInfo().centerCircleRadius)
							continue;
					}
					valid = false;
				}
			}
			else{
				if (i->second.y < Config::getFieldInfo().center.y){
					if (secondGoalOwners == executors){
						Vector2d modelPos(i->second.x, i->second.y);
						if ((modelPos - Config::getFieldInfo().center).length() < Config::getFieldInfo().centerCircleRadius)
							continue;
					}
					valid = false;
				}
			}
		}
	}

	return valid;
}

void AutoReferee::multicast(){

	ptime now = second_clock::universal_time();
	time_duration diff = now - lastMulticastTime;

	if (diff.total_milliseconds() < MULTICAST_PERIOD) return;

	lastMulticastTime = now;

	LOG4CXX_DEBUG(logger, "Sending command: "<<currentCmdPacket.cmd<<" [multicast]");
	server.send(currentCmdPacket);
}

void AutoReferee::send(){
//	currentCmdPacket.cmd_counter++;
	LOG4CXX_DEBUG(logger, "Sending command: "<<currentCmdPacket.cmd);
	server.send(currentCmdPacket);
}

Position2d AutoReferee::getPosition(ModelId id){
	Videodata d = modelsData[id];
	return Position2d(d.x, d.y, d.rot);
}
